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Abstract 

An efficient method for computing the minimum distance and predicting colli- 
sions between moving objects is presented. This problem has been incorporated 
[Kyriakopoulos 90b, 91] in the framework of an in-line motion planning algorithm to 
satisfy collision avoidance between a robot and moving objects modeled as convex 
polyhedra. In the beginning the deterministic problem, where the information about 
the objects is assumed to be certain is examined. If instead of the Euclidean norm, 
Li or Loo norms are used to represent distance, the problem becomes a linear pro- 
gramming problem. The stochastic problem is formulated, where the uncertainty is 
induced by sensing and the unknown dynamics of the moving obstacles. Two prob- 
lems axe considered: First, filtering of the minimum distance between the robot and 
the moving object, at the present time. Second, prediction of the minimum distance 
in the future, in order to predict possible collisions with the moving obstacles and 
estimate the collision time. 

Keywords: Collision Avoidance, Collision Prediction, Distance Functions, Ran- 
dom Search. 


1 INTRODUCTION 

The trajectory planning problem for navigation of mobile robots or manipulation of 
robotic arms has been traditionally treated for environments of stationary or deter- 
ministically moving objects [Brooks 86, Chen 88, Kant 88, Khatib 85, Kyriakopoulos 
88, Lozano-Perez 87, Lumelsky 87, Wu 88]. This is done because in order to satisfy 
task constraints and optimize over certain criteria, all the available apriori informa- 
tion has to be utilized, and therefore most of the planning has to have a global 
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character. 

However, robots should be able to perform their tasks efficiently, not obstructed 
by and not obstructing other moving robots, vehicles or mechanisms with which they 
have loose communication. Therefore, local decision making strategies should be 
included in the menu of the robotic motion planning algorithms. 

The difference between the global and local motion planning strategies is mainly 
found at the amount of processed information about the task and the environment 
and at the considered time length of the plan. Both of these measures are significantly 
larger for the global case which is bound to be off-line under the state of the art of 
hardware. 

An on-line strategy should then decrease the amount of information processing. 
Therefore it should be based first on the already processed information which has ob- 
tained the form of an off-line plan, and second on some additional local measurements 
and estimates of measures related to the obstructing moving objects. Such measures 
have been proposed [Kyriakopoulos 90, 90b, 91] to be the minimum distance be- 
tween the robot and the moving obstacles, and the expected collision time under 
the current plan. 

The calculation of both the minimum distance and the expected collision time 
has been investigated in the past [Canny 86, Gilbert 87, Gilbert 89, Kyriakopoulos 
89] but in the case where full information about the description of the objects and 
their motion is available. Unfortunately, this is not the case in realistic environments 
where the information is based on measurements by a sensing system and the motion 
of the objects is not known but just observed. The uncertainty introduced by both 
factors should be included in the calculation procedures. 

In this paper, the major contribution is the treatment of uncertainty introduced 
by both the sensing process and the unknown attitude of the moving obstacles.The 
distance estimation problem is posed as a filtering problem. Finally, the issue of colli- 
sion time prediction is covered in depth. Such a process is computationally expensive. 
If the L x or L 0 0 are used, then the distance problem is linear programming as opposed 
to quadratic for the case of the Euclidean norm. Linear programming for problems 
of moderate complexity, such as the one at hands, performs considerably better than 
quadratic. 

A statement of the problem in the deterministic case is given in section 2 in order 
to introduce the framework under which analysis is going to be performed. Section 3 
contains the problem formulation of the stochastic case and a proposed solution for 
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the minimum distance estimation. The collision time prediction is given in Section 
4, while some example case with simulation results is presented in Section 5. Finally, 
Section 6 includes comments on the obtained results, and some discussion on issues 
that could constitute topics of future research. 

2 DISTANCE FUNCTIONS - THE DETERMIN- 
ISTIC CASE 

2.1 Definitions 

The distance between two objects at the time instant t is defined [Gilbert 85] as 

d(t) = min{||s,- — Zj\\ : z, € Ki(t),Zj € Kj(t)} (2.1) 

where K{(t), Kj(t) are compact sets of cartesian points representing the two objects. 

Based on the above definition the necessary and sufficient condition so that no 
collision occurs between the two objects would be 

d° - d(t ) <0 Vt 

where d° > 0 is a safety constant. 

Sets Ki(t),Kj(t ) are described by an equation of the form 

K(t) = R(t) ■ C + {T(t)> (2.2) 


where 

C C 9R 3 , is a compact set and describes the shape of the object 
R(t) € Sfc 3 * 3 , is a rotation matrix 
T{t) € 9ft 3 is a translation vector. 

The basic assumption adopted in this work [Kyriakopoulos 89] is that set C rep- 
resenting the shape of the object is a convex polyhedron described by 

C = {x/z e Si 3 3 A ■ x < A € » mx3 ,6€ 3T}. (2.3) 

A point x € S? 3 after it is rotated and translated by R(t) and T(t) respectively, it 
comes to a new point x' where 


x' = R(t) ■ x + T(t). 
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Therefore K{t) since it comes from rotation and translation of points x € C can be 
described as 


K{t) = {x/x eti 3 3A-R-\t)-x<b-A-R- 1 {t)- T(t)}. 

2.2 Representations of distance 

Based on definitions (2.1) and (2.3) and on equation (2.2), the problem to find the 
minimum distance between two solids 

C r = {x/x € a* 3 3 A r ■ x < b r , A r e £ mx3 ,6 r € 3T} 

C 0 = {y/y€^ 3 3A o -y<b o , A> € 8* x3 ,6 0 € £'}. 

representing the robot and an obstacle respectively, is recasted to a mathematical 
programming problem of the form: 

d(t) = min^j, j|x-y|| 

s.t A r (t ) • x(t) < b r (t ) 

Ao{t) ■ y(t) < b 0 (t) 

where 

Ar(t) = Ar • R~ l {t) 
b r (t) = b r -A r -R; 1 (t).T r (t) 

A 0 (t) = A 0 ■ R^it) 
b 0 (t) = b 0 -A o R; l {t).T o (t) 

and Rr(t),T r (t), R o (t),T 0 (t) are the rotation and translation matrices for the robot 
and the obstacle describing their current configuration. In view of (2.4) and (2.5), 
the distance between the solids C T and C Q can be viewed as a function of their 
configuration [Gilbert 85] described as a quadruple P = (Ro(t),T 0 (t), Rr(t),T r (t)): 

d = d(P) : V = 3f^ x3 x a 3 x a 3 * 3 x S 3 — £ + (2.6) 

and therefore a domain V + of d can be defined as: 

V + = [P € V : d{P) > 0} (2.7) 

The norm used in mathematical program (2.4) was not explicitly defined here. It 
can be any norm because norms in 3R n are equivalent. Although the Euclidean norm 
is the most natural to our intuition, other norms such as || • ||i and || ■ ||oo can also 


(2.4) 


(2.5) 
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be used because of their computational efficiency. This comes from the fact that the 
computation of the Euclidean norm requires the solution of a quadratic programming 
problem, while this of || • ||i and || • ||oo requires solution of linear programming prob- 
lems. An analysis of the above issues is presented in appendix A [Kyriakopoulos 89]. 
In the analysis to follow, the minimum distance is going to be associated with no 
specific norm. In case that the kind of norm is significant then it will be indicated 
with the form of a subscript e.g d\, d 2 , d^. 


3 DISTANCE FUNCTIONS - THE STOCHAS- 
TIC CASE 

3.1 Sources of uncertainty 

From (2.4) and (2.5) becomes clear that the computation of the distance between 
two objects requires accurate knowledge of the parameters describing the shape of 
the objects C a and C T and these describing the configuration of them as represented 
by the quadruple P. Accurate knowledge of the solid models C Q and C r , and of the 
configuration parameters Rr(t),T r (t) of the robot is assumed here. The uncertain 
information considered here is the information about the configuration Ro(t),T 0 (t ) of 
the moving obstacle C 0 . Future work is going to incorporate uncertainties in a more 
general fashion. 

The uncertainty of the kinematic information of the moving obstacle C 0 is partially 
due to the way that it is extracted. For example, in a measurement process based 
on a vision system the uncertainty is mainly introduced by two kinds of noises: first, 
the image intensity noise from the environment, and second, the quantization noise 
of the image array of the camera. 

The most eminent source of kinematic uncertainty is the lack of knowledge about 
the “attitude” of the moving obstacle. In other words the lack of knowledge about the 
time evolution of its cartesian position and orientation and their first and second order 
time derivatives. The assumption made here is continuity of position and orientation 
and their velocities. The accelerations were assumed to be constant in time during 
every sampling interval and their magnitude is going to be estimated. 



3.2 Estimation of kinematic parameters 

Naturally, the assumptions made about the models of uncertainty are necessary in 
order to enable the creation of models of motion of the moving object, and the sensing 
process. The models presented here are for the two dimensional case. Future work 
is going to incorporate the full 3-D case which adds complexity just to the formulas. 
On the other hand, the 2-D case perfectly serves the navigation problem considered 
here. 

The equations of motion of the moving obstacle are : 

Px = V x py = Vy 9 = V 9 

v x = a x v y — a y vg = ag (3.1) 

Q r = 0 Cty — 0 OCg = 0 


where 

p x , p y , pg define the cartesian position and orientation in 2-D 
v x , Vy, vg are the translational and rotational velocities and 
a x ,a y ,ag are the translational and rotational accelerations. 

The above equations constitute a set of state equations, and can be written in a 
matrix form: 

X = A • X (3.2) 

where X — \p x v x a x p y v y a y 6 vg a«],A 6 3£ 9x9 . 

The measurement model is described by an equation of the form: 

z = h( X) + v (3.3) 

where z € 3? m is a vector of parameters of several geometric features of the object. 
These parameters are extracted by the sensing process (e.g vision). The dependency 
of z on X is given by the, nonlinear in general, function h(X). The corrupting noise v 
is assumed to be white and Gaussian, and independent of X . TJ»e assumption about 
the Gaussian property of the noise although not accurate, is attributed to the Central 
Limit Theorem, because the noise under consideration is as an aggregation of many 
independent sources of noise [Durrant 88]. The mean and the covariance matrix of 
noise are respectively 

E{v} = 0 

C(v{, Vj) = E{vi ■ Vj ) = Ri • Sij. 
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A variation of the Kalman filter proposed [Athans et.al 68] may well be used to 
get a close to optimal estimate X of state vector X. The selection of this specific 
filter was done because it is a second order filter which normally gives better results 
in terms of bias errors. The utilization of Kalman filter provides, at every instant U : 

• X(U/ti), the optimal estimate of state X based on measurements up to and 
including t,,and 

• the error covariance matrix. 

The prediction equations for the future, based on the information up to and in- 
cluding are 

X(t/t<) = . X(t, ;/U) t>U 

>((/!,) = A ■ P(t,/t.) + P(U/t.) -A t ' 

The above Ricatti equation does not induce any kind of computational difficulty 
because it is homogeneous and can be solved analytically. 


3.3 Bounds of distance functions 


From (2.6), it becomes evident the fact that the dependency of d on quadruple P = 
(-Ro(i), T 0 (t), Rr(t),T r (t)) implies its dependence on the motion vector X. This comes 
from the fact that R 0 {t) and T 0 (t) explicitly depend on X. In fact, in the 2-D case: 


Ro(t) = R 0 (B(t)) = 


cos8 — sind 
sind cosO 


(3.5) 


T a (t) = T 0 (p x {t),p y (t)) = 


Px 

Py 


(3.6) 


Assuming that R r (t) and T r (t) are well known at t, the distance d becomes a function 
of the random variable X, or : 


d = d(X) 


and as a matter of fact d is a random variable, too. From the discussion at the end 
of section 2.2 it becomes obvious that d(X) is not given in a closed form, but it is 
an outcome of a mathematical programming stage. Additional investigation [Clarke 
83, Gilbert et.al 85] has shown that d although uniformly Lipschitz continuous, is 
not differentiable everywhere on its domain V + defined by (2.7). However, V + can 
be viewed as a subset of SR 11 and by Rademachers theorem [Clarke 83], function 



d{X) is differentiable almost every where (a.e), i.e except on a subset of points with 
Lebesgue measure zero. From the geometry of the problem, we can easily see that 
the set of points of nondifferentiability is not dense on and therefore there exist 
open neighborhoods where d(X) is differentiable everywhere and analytic. In the 
discussion to follow, this is going to be mathematically more evident. 

As a matter of fact, function d(X) can be approximated by a Taylor expansion 
up to the second order (due to the Gaussian noise) around the expected value X of 
X: 

i(X) « d(X) + (X - X) T ■ + ~(X - Xf ■ -(X-X) (3.7) 

Taking expected values in both sides of (3.7), 

E(d(X)) > d(X) + • tr(P(t)) (3.8) 

where A min (-) denotes the minimum eigenvalue of a matrix. 

In order to make possible the utilization of (3.8), the Hessian of d(X) has to be 
explicitly found, in order to get the eigenvalues in closed form. This would be more 
efficient for an on-line distance estimation scheme. 

3.4 Derivatives of distance functions 

The analysis that follows considers the 2-D case and is consistent with the develop- 
ment of [Gilbert et.al 85]. The 3-D extension is straightforward. 

Define p a : V + x C 0 x C T — ► R + by: 


T} a (P, W 0 , W T ) = ||i?o -W 0 + p 0 - Rr-W r - p r || 0 
where a defines the kind of used norm. Let 


(3.9) 


L = 

Case a = 2 : In this case 


L x 

L v 


= R 0 -lV 0 +p 0 - Rr-Wr ~p r 


T}2 (P, Wo,W r ) = {L\ + Lj) 1/2 


and 


Vx, h (P, w 0 t w r ) = (^, 0,0, it, o.o. zMiJztJiLh., 0,0) T 

72 72 72 


(3.10) 
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Figure 1: Singular Configuration 

where 

M x = w* ■ sin9 + w y • cosd (3-11) 

M x = w* 0 • cosO — w y • sinO (3-12) 

w 0 = [w* w y 0 f 

the Hessian can be found by further differentiation and is given in appendix B with 
its eigenvalues. 

Based on [Clarke 75, Gilbert et.al 85] it can be proved that in the case that P is 
nonsingular i.e when 

W{P) = {(tu 0 , tu r ) € C 0 X a : r, 2 (P, w 0 ,w r ) = d(P)} 
is singleton, then 

v x d(X) = v x 72 (P, t»;) 

where 

(v>;,w;)€W(P). 

A typical case of singular configuration is demonstrated on figure 1. 

Case,-fl_= ,1: In this case 

7 i(P,u? 0 ,u; r ) = \L X \ + |£ y | (3.13) 
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and 


o_ 

Vx1i(P,u>=,ti) r ) = (5^n(Z. x ),0,0 t 3jn(L v ),0,0, -^-,0, 0) T 


(3.14) 


where 

^ = -(w x sgn(L g ) + w y 0 sgn(L y )) • sinO + ( w*sgn(L y ) - v%sgn(L x )) • cosO (3.15) 

Similarly, the Hessian is very easy to obtain and is given in appendix B with its 
eigenvalues. 

In appendix C the following theorem is proved: 

Theorem 3.1 :If 

d,(P) = minfll* - 2i ||, : € K,(P), z i € K,(P)) 


TJl {P, ™o, Wr) = ||-R 0 • W 0 + p 0 - Rr • W r - p r ||i 


and 


W(P) = {(u; 0 ,u; r ) € C 0 x C T : T}i(P,w 0 ,w r ) = d x (P)} 
is singleton, then 

v x d(j x) = ^xm(P,w;,w;) 

where 

€ W(P). 

Case a = oo : In this case 


Vo°(P,w„Wr) = max{|£»|, |X„t} 


(3.16) 


and 


V x r?oo(-P,^ 0 , t/) r ) = < 


sgn(L x ), 0 , 0 , 0 , 0 , 0 , ^., 0,0 
0 , 0 , 0 , sgn(Ly), 0 . 0 , 0, 0 


if VooiPfWo, w r ) — \I J x\ 
if »?oo(-P, w 0 , w r ) = \L y \ 


where 


dju 

d6 


-{ 


sgn(L x ) • {— tu*cos0 — u;£sin0} if T) oo (P,w 0 ,w r '} = | L 
sgn(L y ) • {w* cos 9 — w y sin 9} if t}oq(P, w 0 , w r ) = | L 


(3.17) 


(3.18) 


The Hessian is given in appendix B with its eigenvalues. 

In appendix C the following corollary is proved using the theorem 3.1 : 
Corollary 3.2 :If 

doo(P) = ipln{||s, - Zjlloo : z, € K t (P ), Zj € Kj(P)} 
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= Ro ■ Wo + Po - Rr • W r - Pr 



T]oo(P,w 0 ,w r ) = ||I|U = max{|Z-. F |,|L y |} 

and 

W(X) = arg min{|Zr r |, |L y |} 

W(P) = {(u; 0 ,u> r ) £C 0 x C T : t}^ (P, w a , w r ) = d^P)} 
are singleton, with W(£) = {£“} and W(P) = {(u;*,u;*)} then 

Vxd(X) = v x r(p,w;,w;). 


3.5 Distance Estimation Algorithm 

In summary, the steps of the distance estimation (prediction) algorithm axe given: 

• KALMAN FILTER provides X(t/ti), P(t/ti) t > t,- ( equation (3.4) ) 

• from a mathematical programming stage d^X) (or d\(X)) is obtained ( equa- 
tion (2.4) ) 

• from one of (B.1),(B.2),(B.3) A min is obtained, and 

• from (3.8) a lower bound for E(d(X)) is obtained. 

4 PREDICTION OF COLLISION TIME 

The prediction of collision time is going to be based on two kinds of information. 
First, the future motion of the robot which is preplanned and therefore accurately 
known. Second, the motion of the moving obstacle, as can be “optimally” predicted 
by (3.4). It is worthwhile noticing the fact that distance, as a function of time, is 
not necessarily differentiable. Additionally, the future time-evolution of d{t) is not 
convex. Finally, we are not interested just in a local mini mum of d(t), but in the first 
time instant t c that d(t) becomes zero, or mathematically, 

(4.1) 

where t, is the present time, and T is the time horizon of interest. 


t c — inf (d(f) = 0} 
leM.+rp v ’ 1 
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The characteristics of the above problem do not permit the use of stochastic 
approximation methods. Additionally some methods that have recently appeared, 
[Gilbert 89] although they give accurately the collision time based on the considered 
information, they seem to have computational problems that do not enable their use 
in on-line schemes. For those reasons, another method, the accelerated expanding 
subinterval random search (AESRS) [Saridis 77] was utilized with modifications 
to accommodate the problem. The convergence properties of this method have well 
been investigated, and its performance experimentally verified. The basic idea of 
AESRS is the following: 

Obtain the lower bound d(U) from (3.8) and set d m in = d(U). Split the interval 
Cl = [tj, f, + T] of concern in N subintervals Cli C Cl such that fi, H Cl j = 0 , i ^ j • 
Define a probability density function (pdf) p(g) over Cl by: 

N N 

p(q) = 5Z = P{k)pk{ek), &k eft*, /?fe = l (4.2) 

*:=i fc=i 

where each Pk(dk) is defined only on D*. The pdf’s Pk(0k) can be chosen to be any 
distribution, but in this work were chosen to have uniform distribution for practiced 
reasons. Initially /3(k ) = (3o(k) = jj. 

From (4.2) a random time instant t r € Cl is obtained, and from (3.8) a lower 
bound d(t r ) is obtained for the distance. If d(t r ) < d min then it is assumed to be a 
“successful” iteration. The weights 0i(k) are updated by a stochastic approximation 
reinforcement algorithm, based on the frequency of “successful” iterations in the 
particular k — th bin, 


d(tr) 


< d m i„ 


> d n 


I ft(fc) = i.-t(fc) + 7.[i - A'-iM] 

\ = + 0 ) i* k 

k — 1, ..,N 


(4.3) 


In the above equations 7 j and fio(k) must satisfy Dvoretzky’s conditions for conver- 


gence 

i » 

lim 7 i = 0, Jim 7 j = Jim J^7 ? < 00, ||A)(fc)|| < 00 

—00 -00^ 

therefore 7 ,• = 7 and fSo{k) = jj. 

The definition of t c in (4.1) suggests that the following amendments to the above 
algorithm should be done to achieve better performance. 

A) Consider figure 2. The present time is f,- = 2sec and the time horizon of 
interest is T* = 6sec. A histogram of N = 12 bins represents Pk(0k), while the dashed 
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Figure 2: d(t/ti = 2) and probability histogram for fi = [2,8] 
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Figure 3: = 2) and probability histogram for Cl = [2,7] 
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Figure 4: d(t/t{ = 2) and probability histogram for ft = [2,6] at time t = 3~. 

line is d{t/U = 2) t € [2 , £,• + 1\]. The lower bound of the predicted distance d(t/ti) 
is computed using (3.8) not everywhere but only at random instants t r produced by 
Pk(Qk)- 

Assume that a trial time instant t r = 7 sec is randomly produced by the probability 
of (4.2). Then d(t r /ti ) = 0. Then a candidate collision time t c = t r = 7 has been 
found and the search interval Q is modified to ft = [<,• = 2,t c = 7]. Then the new 
search interval is being divided in N subintervals and therefore a finer grid is obtained 
(figure 3). Since the grid becomes finer and finer the uniform distribution for pjt(^Jt) 
is satisfactory. The shape of the distribution of {3(k ) k = 1, N in the new interval, 
is similar to this of the previous distribution providing the search with “memory”. 

B) Consider figure 4. The present time is t = 3~, all the information processing 
has been done based on sensing info at t, = 2sec and the time horizon of interest is 
T k = 4sec. A histogram of N = 8 bins represents pk{Qk ), while the dashed line is 
d(t/ti = 2) t € [2, ti + 7a]. 

When a new measurement d(t 1+1 = 3) arrives (figure 5), the distribution of fl{k) 
for the bins that available from the previous search (i.e [3,6] in figure 4), is not 
changed because due to the Lipschitz continuity of d(-) w.r.t. <f>, small changes of 
4>(tilti) produce, bounded changes of d{t r /ti). The distribution over [f,-,t,'+i] (i.e of 


14 





Figure 5: d(t/ti = 2) (•••), d(t/ti = 3) ( ) and probability histogram for fi = [3, 7] 

at time t = 3 + . 

time interval [2,3] in figure 4) is uniformly distributed along the new extension (i.e 
time interval [6,7] in figure 5). 

In this search d\ was used, as measure of distance, because it is computationally 
faster, while it gives the same collision time as d 2 . 

Finally an issue of interest is the selection of the time horizon T of search. It 
should be based on a number of criteria depending on safety and performance issues. 
In this work a reasonable and safe selection was considered: 

r = n-^±il, V = )-<*(!, )| 

where n is a positive integer. 

5 SIMULATION RESULTS 

The case study considers a mobile robot endowed with a vision and motion control 
system and a moving obstacle (figure 6). The vision system “traces” the edges of 
the moving polyhedron representing the obstacle, and the parameters of their math- 
ematical description, constituted the measurement vector z of equation (3.3). The 
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Figure 6: Environment with a mobile robot and a moving obstacle 

measurement noise v was assumed to be white, Gaussian, with zero mean and diag- 
onal covariance matrix with diagonal elements equal to 0.1. 

The Kalman filter output behaved well especially in the straight line motions. The 
lower bounds of E(di(X)) calculated by (3.8), are plotted in figure 7(a), while the 
error is plotted in figure 7(b). As expected, the lower bound underestimates and is 
close enough to the actual distance. The maximum absolute error is 7cm in a range 
of actual distances of 20m. 

The collision time was calculated using the modified AESRS that was outlined in 
the previous chapter. As a metric, d\{t) was used. Therefore using a SUN 3-150 we 
were able to perform 30 iterations/sec. At time instants t = 4,5,6 sec the collision 
time was found to be t c = 7, 6.8, 6.69 sec respectively, while the actual collision time 
is t c = 6.63 sec. 


6 DISCUSSION - FUTURE RESEARCH 

A theoretical analysis of the distance estimation and collision time prediction prob- 
lems between moving polyhedra was presented along with simulation results. The 
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whole analysis was based on an uncertainty model including sensing and attitude un- 
certainty. The computational efficiency resulting from the use of L\ and metrics 
and the heuristic amendments to AESRS was verified with a simulation study. Future 
research should include application of the above results in real time systems. Such 
an effort is currently in progress in the NASA-CIRSSE Laboratories of RPI. 

The above analysis should be extended for the case of nonconvex objects that can 
be decomposed to a number of convex ones. Further investigation is also needed for 
the calculation of the collision time. Although the modified AESRS performed well, it 
really makes big use of CPU. Therefore, either some parallelizable scheme of AESRS 
should be developed. 

Decision schemes for collision avoidance, using the information calculated here in 
the framework described in the introduction have been reported [Kynakopoulos 90b, 
91]. Further applications are under development and results are going to be reported 
in the near future. 


APPENDIX A 

In (2.4) the distance computation problem between two convex polyhedra was 
stated as the mathematical programming problem: 


d = min XiV ||x - y|| 0 
s.t A r • x < b r 
A 0 y <b 0 

where integer a defines the kind of sought distance, x,y € 5ft 3 , A r € S mx3 ,6 r € 3R m , 
A 0 € 5ft ,x3 , 6 0 6 5R ; . Integers m,l axe the numbers of the sides of the polyhedra 
representing the robot and the obstacle respectively. 

If we set z = [x T y T ] T , L = [bj bg] T , D = [/ 3 X 3 —hxz] (I is identity matrix), 

, r A r 0 m x3 
M — 

0/X3 A> 

then the above problem is written in the standard mathematical programming form 


d = min- ||D • z\\ a 
s.t M ■ z < L 


(A.l) 


q = 2 

Mathematical programming problem (A.l) is equivalent to 

d = min* z T • Q • z 
s.t M • z < L 
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with Q = D T • D € Sfc 6 * 6 , and becomes a quadratic programming problem. Q is 
positive semidefinite. Therefore more than one minimizing vectors may exist, with 
all of them corresponding to the same minimum. 
a = 1 

If a new vector of unknowns w € 3R 3 is defined 

Wi = i Di • z\ = |x ; - y,| 

with Di the i-th row of D, then the mathematical programming problem (A.l) is 
equivalent to 

d = min z , w £? =1 1 a, 
s.t Wi > D{ ■ z i — 1,2, 3 
Wi > — Di ■ z i = 1,2, 3 
M ■ z < L 

and becomes a linear programming problem of 9 variables and m -f l + 6 constraints. 
More than one minimizing vectors may exist, with all of them corresponding to the 
same minimum. 
a = oo 

If a new unknown w € 32 is introduced, such that 

w = max I Di • z\ = max lx, — y,-| 

>= 1 , 2 , 3 1 1 (= 1^3 

with Di the i-th row of D, then the mathematical programming problem (A.l) is 
equivalent to 

d = min z ,u > w 
s.t w > Di • z z = 1,2,3 
w > —D{ ■ z i = 1, 2, 3 

M ■ z < L 

and becomes a linear programming problem of 7 variables and m + / + 6 constraints. 
More than one minimizing vectors may exist, with all of them corresponding to the 
same minimum. 

APPENDIX B 

In this appendix, the derivation of the eigenvalues of the Hessian of the distance 
function is presented for two cases of norms. 

a = 2 : If G = M x L y + M y L x then direct differentiation of (3.10) gives: 

■ L\ -L x L y -LyG 

-L x Ly LI L X G 

. -LyG L X G G(G - Til) . 


d 2 rte(X) _ 1 
d.X* ' 
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(B.l) 


Setting T = —Grjj and B = rjl(G — 1) — G 2 , 

■d^X) 




4 «r)t :fl 


; - 2 nt 


a = 1 : Direct differentiation of (3.13) in conjunction with (3.15) gives: 


d^{X) 

d X 2 


0 0 0 
0 0 0 



where 

= -( w x 0 sgn(L x ) + w v 0 sgn(L y )) • cos0 - (u£s 0 n(£ v ) - u^n(Zr r )) • stnd 

ou t 

and therefore „ _ 

Amm( qq 2 )* ^ ^ 

a = oo : Direct differentiation of (3.16) in conjunction with (3.18) gives: 


gg oo(j0 

dX 2 


0 0 0 
0 0 0 
0 0 ^ 


where 


7oo 

aa 2 

and therefore 


sgn(L x ) • {u£sin<? — iw"cosfl} ^(P.w,,^) = |-£ r [ 
sgn(Ly) • {— iwj sin 0 - uj"cos0} ^(P, ta 0 , u; r ) = |Z-*| 



gVo(jO 

dX 2 


) 


rm’n{0, 


d^oo -, 

aa 2 ' 


(5.3) 


1 


APPENDIX C 

Some definitions, theorems and lemmas that are going to be utilized in the proof 
of the theorem are presented here. Most of those results are based on [Clarke75] 
[Clarke83] [Gilb85]. 

Consider / : O — ► 3i, be locally Lipschitz, where O C 3? n . We say that / admits 
a Frechet derivative V/ at x 6 O if 


lim 

5—0 


/(x + <?x) - /( x) 

Ill'll " 


=< V/, Sx > 
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where <, > denotes the inner product. The usual (one-sided) Directional deriva- 
tive of / at x in the direction v is 


f'{x-,v) = lim 


/(.r + tv) - /(x) 
t 


The Generalized gradient of / at x is 


df(x) = co{w : V/(x<) -*• w, V/(x;) exists , x { -*• x) 

where co denotes the convex hull. Finally the Generalized directional derivative 
/°(x;u) is defined as: 


f°(x-,v) = 


lim 


y — ► x 
t | 0 


f(x + tv) - f{x) 

sup 


Definition C-l: A function f(x) is said to be regular at x provided: 

(i) f'(x\ u) exists Vu. 

(ii) f(x ; v ) = f°(x ; v ) Vu. 

Proposition C-2: Let / be Lipschitz near x then / is regular at x. 

Proof: The proof is given in [Clarke83]( proposition 2.3.6).|. 

Proposition C-3: Let / : X — * 3?, with X finite dimensional. Then df is upper 
semicontinuous Vx € X. 

Proof: The proof is given in [Clarke83]( proposition 2.1.5).|. 

The following theorem is key to our development 

Theorem C-4: Let U be a sequentially compact space, and let g : 3£ n x U ^ » 
have the following properties: 

(a) g(x,u) is upper semicontinuous in (x, u). 

(b) g is locally Lipschitz in x, uniformly for u in U. 

(c) £°(x,u; •) = g' x (x,u-, •), the derivatives being w.r.t x. 

(d) d x g(x,u ) is upper semicontinuous in (x.u). 

If we let /(x) = max u€ y{5(x,u)} then : 

(1) / is locally Lipschitz. 

(2) '/'(x;u) exists. 

(3) /'(x;u) = /°(x; u) = max{C • v : C € ^(x,^),^ G Af(x) }, where M(x) = {tx € 
U : g{x,u) = /(x)}. 

(4) df(x) = co{d x g(x,u ) : u € M{x)}. (co{ ) denotes the convex hull of a set). 
Proofs The proof is given in [Clarke 75] (Theorem 2.1)j. 



Finally, the following proposition is the last piece of the proof under development. 
Lemma C-5: The following are equivalent: 

(a) df{ x) = {C}, is a singleton, (b) V/(. r) exists, V/(x) = C, and V/ is continuous 
at x relative to the set upon it exists. 

Proof: The proof is given in [ClarkeTo] (Proposition 1.13).|. 

Lemma C-6: The following results are true for 

7 h{P, w 0 , W r ) = ||P 0 ■ W 0 + p 0 - Rr ■ W r - p r ||i 

a.)r]i(P, w 0 ,w r ) is continuous and convex on its domain. 

b) r7x is Locally Lipschitz. 

c ) v°p{P, w 0 , w r \ •) = Tj[p(P, Wo , W T ; •) 

d ) dpJh(P,w 0 ,w r ) is upper-semi-continuous. 

Proofs (a) and (b) can be easily derived from the definition of 771 . 

(c) From C-2 rfo is regular, and from definition C-l QED. 

(d) The domain of r)i can be viewed as a subset of 3fc n . From proposition C-3 the 
sought result is obvious. |. 

Proofof Theorem 3.1 : Lemma C-6 in conjunction to theorem C-4 gives the fol- 
lowing results for function 

di(P) = min{T)i(P,w 0 .u! r ) : w 0 € C Q , w r € C r } 


1) ^! is locally Lipschitz. 

2) di'(P] P') exists. 

3 ) di(P]P') = min{< F >: ( w Q ,w T ) 6 W(P)} where 
W(P) = {(uj 0 ,rw r ) eC 0 xC r : ^(P, w 0 , w T ) = d x (P)} 

4) ddi(P) = co{< V P T]i(P,Wo,w r ),P' >: {w„, w r ) € W(P)}. 

From lemma C-5 the following statements are shown to be equivalent: 
ddi(P) = {^}, singleton; Vd a (P) exists; '7d 1 (P)=£. 

Result 

When the configuration of C 0 and C T is such that there is only one pair of points 
(w’~, w*) minimizing di(P) (i.e when W(P) is singleton) then according to the above 
results Vpdx(P) = Vprj 1 (P, w’, w"). Q.E.D.|. 

Before continuing with the proof of corollary 3.2 the following lemma is proved: 
Lemma C-7: Let fi ,/2 : X -+ 91. with X finite dimensional. If /1, f* are 
continuous, convex and Lipschitz continuous then the following results are true for 
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function / : X —* 3? defined by 

f(x) = max{/ 1 (i),/ 2 (i)} 

a) / is continuous and convex on its domain. 

b ) / is Locally Lipschitz. 

c )/£(*;•) = £(*;•) 

d )d x J{x) is upper-semi-continuous. 

Proof: 

(a) The coninuouity of / is obvious. Consider x € X such that x = A ■ xx -I- (1 — A) • x 2 
where xx,x 2 6 X. Then f(x) = /,(x) i € {1,2}, but since fi is convex, 

/ (A • xi + (1 — A) • x 2 ) = /(x) = fi(x) < A ■ fi{xi) + (1 — A) • fi(x 2 ) 

but since f(xj) > fi(xj) i,j € {1,2}, 

/ (A • xx + (1 - A) • x 2 ) < A • /(x 1 ) + (1 - A) - /(x 2 ). 


Thus / is convex. 

(b) Since /,- is Lipschitz then there exists constant c,- > 0 such that 

|/,(x) - fi(y)\ < Ci\\x - y\\ x Vx, y € X (C.l) 

where || • ||x is the induced norm of space X. Assume that 


/{*) = f,(x) (C2) 

f(y ) = fAy) (C- 3 ) 

if i = j then /,(y) = fj(y). Otherwise, fj(y) > /;(x) (from C.2). Therefore, in general 


My) - fAy) < 0 (C.4) 

Consider now, 

tr (C.4) 

/(*) - f(y) ' = ’ /.(*) - !,(v) = /iW - fAy) + My) - fAy) £ !A Z ) - fAy) 

therefore 

\f(x) - f(y)\ <\fi(x) - fi{y)\ < Ci||x-y||* *€{1,2} 
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and 

\f{x)-f{y)\<c\\x-y\\x c = max c, 
therefore / is Lipschitz. 

(c) & (d) are proved the same way as in lemma C-6 j. 

Proof of Corollary 3.2 : We follow the same steps as in the theorem of Theorem 
3.1. 

From Lemma C-7, Theorem C-4 and Lemma C-5 we come to the conclusion that 
when the configuration of C 0 and C T is such that there is only one pair of points 
(iu*,uj") minimizing d^P) (i.e when W(P) is singleton) then 

v F d 0O (P) = v pnoo (P,w;, <). (c.5) 

Similarly, when L(P, w“, w*) = [L x L y ] T is such that only one of L x , L y (denote it 
as f) maximizes t)(P) (i.e when W(L) is singleton) then 

v PVoo (P,w;,w' r ) = v P r{P,wi,w m r ). (c.6) 

From (C.5, C.6) the following result is obtained 

v p <up) = v P r(PX,u>;). 
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